/********************************************************************
	filename: 	GeoAlgorithm2D.cpp
	created:	2010/04/16
	author:		Xiuwei Chen

	purpose:	Implement the function of 2D geometry algorithm

	All rights Reserved
*********************************************************************/

#include "GeoAlgorithm2D.h"


/************************************************************************/
/* Equal                                                                */
/************************************************************************/

/// <Summary>
/// check if two numbers are equal
/// </Summary>
bool Equal(double a, double b)
{
	return fabs(a - b) <= EP;
}

/// <Summary>
/// check if two 2D Point is equal
/// </Summary>
bool Equal(const Point2D &p1, const Point2D &p2)
{
	return fabs(p1.x - p2.x) <= EP && fabs(p1.y - p2.y) <= EP;
}

/// <Summary>
/// check if two 2D line is equal
/// </Summary>
bool Equal(const Line2D &line0, const Line2D &line1)
{
	return Parallel(line0, line1) && 
		fabs(line0.c - line1.c) / sqrt(line0.a * line0.a + line0.b * line0.b) <= EP;
}

/// <Summary>
/// check if two 2D Segment is equal
/// </Summary>
bool Equal(const Segment2D &seg0, const Segment2D &seg1)
{
	return (Equal(seg0.p0, seg1.p0) && Equal(seg0.p1, seg1.p1)) ||
		(Equal(seg0.p1, seg1.p0) && Equal(seg0.p0, seg1.p1));
}

/// <Summary>
/// check if two 2D rectangles are equal
/// </Summary>
bool Equal(const Rectangle2D &rect0, const Rectangle2D &rect1)
{
	return Equal(rect0.x0, rect1.x0)
		&& Equal(rect0.x1, rect1.x1)
		&& Equal(rect0.y0, rect1.y0)
		&& Equal(rect0.y1, rect1.y1);
}


/************************************************************************/
/* Distance                                                             */
/************************************************************************/

/// <Summary>
/// distance square of two 2D point
/// </Summary>
double DistanceSquare(const Point2D &p1, const Point2D &p2)
{
	return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y);
}

/// <Summary>
/// distance of tow 2D point
/// </Summary>
double Distance(const Point2D &p1, const Point2D &p2)
{
	return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
}

/// <Summary>
/// distance between 2D point and 2D line
/// </Summary>
double Distance(const Point2D &p, const Line2D &line)
{
	return fabs(line.a * p.x + line.b * p.y + line.c) / sqrt(line.a * line.a + line.b * line.b);
}


/// <Summary>
/// distance between two 2D line [EP]
/// </Summary>
double Distance(const Line2D &line0, const Line2D &line1)
{
	if(Parallel(line0, line1) == false) return 0;
	return fabs(line0.c - line1.c) / sqrt(line0.a * line0.a + line0.b * line0.b);
}

/// <Summary>
/// distance between 2D point and 2D segment
/// </Summary>
double Distance(const Point2D &p, const Segment2D &seg)
{
	double ret = std::min(Distance(p, seg.p0), Distance(p, seg.p1));
	Point2D pt = Nearest(p, Line2D(seg.p0, seg.p1));
	if(Inside(pt, seg) == true)
	{
		ret = std::min(ret, Distance(p, pt));
	}
	return ret;
}

/// <Summary>
/// distance between two 2D segments
/// </Summary>
double Distance(const Segment2D &seg0, const Segment2D &seg1)
{
	if(Intersect(seg0, seg1) == true)
	{
		return 0;
	}
	else
	{
		return std::min(Distance(seg0.p0, seg1), Distance(seg0.p1, seg1));
	}
}

/// <Summary>
/// distance between a point and a rectangle
/// </Summary>
double Distance(const Point2D &p, const Rectangle2D &rect)
{
	return Distance(p, Nearest(p, rect));
}

/// <Summary>
/// distance between two rectangles
/// </Summary>
double Distance(const Rectangle2D &rect0, const Rectangle2D &rect1)
{
	double dx, dy;
	if(rect0.x0 < rect1.x0)
	{
		dx = std::max(0.0, rect1.x0 - rect0.x1);
	}
	else
	{
		dx = std::max(0.0, rect0.x0 - rect1.x1);
	}
	if(rect0.y0 < rect1.y0)
	{
		dy = std::max(0.0, rect1.y0 - rect0.y1);
	}
	else
	{
		dy = std::max(0.0, rect0.y0 - rect1.y1);
	}
	return sqrt(dx * dx + dy * dy);
}

/************************************************************************/
/* CrossProduct and DotProduct                                          */
/************************************************************************/

/// <Summary>
/// cross product of two 2D vector
/// </Summary>
double CrossProduct(const Vector2D &v1, const Vector2D &v2)
{
	return v1.x * v2.y - v1.y * v2.x;
}

/// <Summary>
/// cross product of two 2D vector: (p1 - p0) and (p2 - p0)
/// </Summary>
double CrossProduct(const Point2D &p0, const Point2D &p1, const Point2D &p2)
{
	return (p1.x - p0.x) * (p2.y - p0.y) - (p1.y - p0.y) * (p2.x - p0.x);
}

/// <Summary>
/// cross product of two 2D vector
/// </Summary>
double DotProduct(const Vector2D &v1, const Vector2D &v2)
{
	return v1.x * v2.x + v1.y * v2.y;
}

/// <Summary>
/// cross product of two 2D vector: (p1 - p0) and (p2 - p0)
/// </Summary>
double DotProduct(const Point2D &p0, const Point2D &p1, const Point2D &p2)
{
	return (p1.x - p0.x) * (p2.x - p0.x) + (p1.y - p0.y) * (p2.y - p0.y);
}

/************************************************************************/
/* Angle                                                                */
/************************************************************************/

/// <Summary>
/// the angle between two 2D line: [0, PI / 2]
/// </Summary>
double Angle(const Line2D &line0, const Line2D &line1)
{
	double ret = fabs(line0.Angle() - line1.Angle());
	if(ret > PI / 2)
	{
		ret = PI - ret;
	}
	return ret;
}

/// <Summary>
/// the counter clockwise angle from 2D vector v0 to v1: [-PI, PI)
/// </Summary>
double Angle(const Vector2D &v0, const Vector2D &v1)
{
	double cos = v0 * v1 / (v0.Norm() * v1.Norm());
	if(cos >= 1.0) return 0;
	if(cos <= -1.0) return -PI;
	if(CrossProduct(v0, v1) > 0)
	{
		return acos(cos);
	}
	else
	{
		return -acos(cos);
	}
}

/************************************************************************/
/* Symmetry                                                             */
/************************************************************************/

/// <Summary>
/// get 2D point's symmetry point about a 2D point
/// </Summary>
GEOMETRY_EXPORT Point2D Symmetry(const Point2D &p, const Point2D &origin)
{
	return Point2D(origin.x + origin.x - p.x, origin.y + origin.y - p.y);
}

/// <Summary>
/// get 2D point's symmetry point about a 2D line
/// </Summary>
Point2D Symmetry(const Point2D &p, const Line2D &line)
{
	double d = line.a * line.a + line.b * line.b;
	double x = (p.x * (line.b * line.b - line.a * line.a) - 2 * line.a *line.b * p.y - 2 * line.a * line.c) / d;
	double y = (p.y * (line.a * line.a - line.b * line.b) - 2 * line.a *line.b * p.x - 2 * line.b * line.c) / d;
	return Point2D(x, y);
}

/// <Summary>
/// get 2D line's symmetry line about a 2D point
/// </Summary>
Line2D Symmetry(const Line2D &line, const Point2D &p)
{
	double c = line.c + 2 * line.a * p.x + 2 * line.b * p.y;
	return Line2D(line.a, line.b, -c);
}

/************************************************************************/
/* Center                                                               */
/************************************************************************/

/// <Summary>
/// center of two 2D point
/// </Summary>
Point2D Center(const Point2D &p0, const Point2D &p1)
{
	return Point2D(p0.x + (p1.x - p0.x) / 2, p0.y + (p1.y - p0.y) / 2);
}

/// <Summary>
/// center of a segment
/// </Summary>
Point2D Center(const Segment2D &seg)
{
	return Point2D(seg.p0.x + (seg.p1.x - seg.p0.x) / 2, seg.p0.y + (seg.p1.y - seg.p0.y) / 2);
}

/// <Summary>
/// center of 2D rectangle
/// </Summary>
Point2D Center(const Rectangle2D &rect)
{
	return Point2D(rect.x0 + (rect.x1 - rect.x0) / 2, rect.y0 + (rect.y1 - rect.y0) / 2);
}

/************************************************************************/
/* Perpendicular and Parallel                                           */
/************************************************************************/

/// <Summary>
/// get the Perpendicular of 2D line which pass by a point
/// </Summary>
Line2D Perpendicular(const Line2D &line, const Point2D &p)
{
	return Line2D(line.b, -line.a, line.a * p.y - line.b * p.x);
}

/// <Summary>
/// check if two 2D lines are parallel [EP]
/// </Summary>
bool Parallel(const Line2D &line0, const Line2D &line1)
{
	return fabs(line0.a * line1.b - line0.b * line1.a) <= EP;
}


/************************************************************************/
/* Nearest Point                                                        */
/************************************************************************/

/// <Summary>
/// get 2D point's nearest point on a 2D line (foot of a Perpendicular)
/// </Summary>
Point2D Nearest(const Point2D &p, const Line2D &line)
{
	double d = line.a * line.a + line.b * line.b;
	double x = (line.b * (line.b * p.x - line.a * p.y) - line.a * line.c) / d;
	double y = (line.a * (line.a * p.y - line.b * p.x) - line.b * line.c) / d;
	return Point2D(x, y);
}

/// <Summary>
/// get 2D point's nearest point on a 2D segment
/// </Summary>
Point2D Nearest(const Point2D &p, const Segment2D &seg)
{
	Point2D ret = seg.p0;
	double d = Distance(p, seg.p0);
	double tmp = Distance(p, seg.p1);
	if(tmp < d)
	{
		d = tmp;
		ret = seg.p1;
	}
	Point2D pt = Nearest(p, Line2D(seg.p0, seg.p1));
	if(Inside(p, Rectangle2D(seg.p0, seg.p1)) == true)
	{
		tmp = Distance(p, pt);
		if(tmp < d)
		{
			d = tmp;
			ret = pt;
		}
	}
	return ret;
}

/// <Summary>
/// get 2D point's nearest point on a 2D rectangle
/// </Summary>
Point2D Nearest(const Point2D &p, const Rectangle2D &rect)
{
	double x = std::min(rect.x1, std::max(p.x, rect.x0));
	double y = std::min(rect.y1, std::max(p.y, rect.y0));
	return Point2D(x, y);
}

/************************************************************************/
/* Inside                                                               */
/************************************************************************/

/// <Summary>
/// check if 2D point is inside the 2D line [EP]
/// </Summary>
bool Inside(const Point2D &p, const Line2D &line, bool bound)
{
	return fabs(p.x * line.a + p.y * line.b + line.c) <= EP;
}

/// <Summary>
/// check if 2D point is inside the 2D segment [EP]
/// </Summary>
bool Inside(const Point2D &p, const Segment2D &seg, bool bound)
{
	if(fabs(CrossProduct(p, seg.p0, seg.p1)) <= EP)
	{
		bool ret = Inside(p, Rectangle2D(seg.p0, seg.p1));
		if(ret == true && bound == false)
		{
			ret = (!Equal(p, seg.p0) && !Equal(p, seg.p1));
		}
		return ret;
	}
	else
	{
		return false;
	}
}

/// <Summary>
/// check if 2D point is inside the 2D rectangle
/// </Summary>
bool Inside(const Point2D &p, const Rectangle2D &rect, bool bound)
{
	if(bound == true)
	{
		return rect.x0 <= p.x && p.x <= rect.x1 && rect.y0 <= p.y && p.y <= rect.y1;
	}
	else
	{
		return rect.x0 < p.x && p.x < rect.x1 && rect.y0 < p.y && p.y < rect.y1;
	}
}


/************************************************************************/
/* Intersect                                                            */
/************************************************************************/

/// <Summary>
/// check if two 2D lines are intersect, get the intersect point if yes.
/// </Summary>
bool Intersect(const Line2D &line0, const Line2D &line1, Point2D *point)
{
	double d = line0.a * line1.b - line1.a * line0.b;
	if(fabs(d) <= EP)
	{
		return false;
	}
	else
	{
		if(point != NULL)
		{
			point->x = (line0.b * line1.c - line0.c * line1.b) / d;
			point->y = (line0.c * line1.a - line0.a * line1.c) / d;
		}
		return true;
	}
}

/// <Summary>
/// check if two 2D Segments are intersect, get the intersect point if yes.
/// </Summary>
bool Intersect(const Segment2D &seg0, const Segment2D &seg1, Point2D *point)
{
	Point2D p;
	if(Intersect(Line2D(seg0.p0, seg0.p1), Line2D(seg1.p0, seg1.p1), &p) == true)
	{
		if(Inside(p, Rectangle2D(seg0.p0, seg0.p1))== true && Inside(p, Rectangle2D(seg1.p0, seg1.p1)) == true)
		{
			if(point != NULL)
			{
				*point = p;
			}
			return true;
		}
		else
		{
			return false;
		}
	}
	else
	{
		return false;
	}
}

/// <Summary>
/// check if 2D segments and 2D line are intersect, get the intersect point if yes.
/// </Summary>
bool Intersect(const Segment2D &seg, const Line2D &line, Point2D *point)
{
	double t = line.Evaluation(seg.p0) * line.Evaluation(seg.p1);
	if(t <= 0)
	{
		return Intersect(Line2D(seg.p0, seg.p1), line, point);
	}
	else
	{
		return false;
	}
}

/// <Summary>
/// check if two 2D rectangle are intersect, get the intersect rectangle if yes.
/// </Summary>
bool Intersect(const Rectangle2D &rect0, const Rectangle2D &rect1, Rectangle2D *rect)
{
	double x0 = std::max(rect0.x0, rect1.x0);
	double x1 = std::min(rect0.x1, rect1.x1);
	double y0 = std::max(rect0.y0, rect1.y0);
	double y1 = std::min(rect0.y1, rect1.y1);
	if(x0 <= x1 && y0 <= y1)
	{
		if(rect != NULL)
		{
			*rect = Rectangle2D(x0, y0, x1, y1);
		}
		return true;
	}
	else
	{
		return false;
	}
}

/// <Summary>
/// check if 2D rectangle and 2D line are intersect, get the intersect segment if yes.
/// </Summary>
bool Intersect(const Line2D &line, const Rectangle2D &rect, Segment2D *segment)
{
	if(line.a == 0)
	{
		double y = -line.c / line.b;
		if(y < rect.y0 || rect.y1 < y)
			return false;
		if(segment != NULL)
			*segment = Segment2D(rect.x0, y, rect.x1, y);
		return true;
	}
	else if(line.b == 0)
	{
		double x = -line.c / line.a;
		if(x < rect.x0 || rect.x1 < x)
			return false;
		if(segment != NULL)
			*segment = Segment2D(x, rect.y0, x, rect.y1);
		return true;
	}
	else
	{
		double y0 = (-line.a * rect.x0 - line.c) / line.b;
		double y1 = (-line.a * rect.x1 - line.c) / line.b;
		if(y1 > y0)
			std::swap(y0, y1);
		if(y0 < rect.y0)
			y0 = rect.y0;
		if(y1 > rect.y1)
			y1 = rect.y1;
		if(y0 < y1)
			return false;
		if(segment != NULL)
		{
			double x0 = (-line.b * y0 - line.c) / line.a;
			double x1 = (-line.b * y1 - line.c) / line.a;
			*segment = Segment2D(x0, y0, x1, y1);
		}
		return true;
	}
}

/// <Summary>
/// check if 2D rectangle and 2D segment are intersect, get the intersect segment if yes.
/// Barsky Algorithm
/// </Summary>
bool Intersect(const Segment2D &seg, const Rectangle2D &rect, Segment2D *segment)
{
	double u1 = 0, u2 = 1.0;
	double p[4] = {
		seg.p0.x - seg.p1.x,
		seg.p1.x - seg.p0.x,
		seg.p0.y - seg.p1.y,
		seg.p1.y - seg.p0.y
	};
	double q[4] = {
		seg.p0.x - rect.x0,
		rect.x1 - seg.p0.x,
		seg.p0.y - rect.y0,
		rect.y1 - seg.p0.y
	};
	for(int i = 0; i < 4; i++)
	{
		if (p[i] == 0)
		{
			if(q[i] < 0) return false;
		}
		else if(p[i] < 0)
		{
			u1 = std::max(u1, q[i] / p[i]);
		}
		else
		{
			u2 = std::min(u2, q[i] / p[i]);
		}
	}
	if(u1 > u2) return false;
	if(segment != NULL)
	{
		*segment = Segment2D(
			seg.p0.x + (seg.p1.x - seg.p0.x) * u1,
			seg.p0.y + (seg.p1.y - seg.p0.y) * u1,
			seg.p0.x + (seg.p1.x - seg.p0.x) * u2,
			seg.p0.y + (seg.p1.y - seg.p0.y) * u2
			);
	}
	return true;
}

/************************************************************************/
/* Translation                                                          */
/************************************************************************/

/// <Summary>
/// 2D Point's Translation
/// </Summary>
Point2D Translation(const Point2D &point, const Vector2D &v)
{
	return Point2D(point.x + v.x, point.y + v.y);
}

/// <Summary>
/// 2D Segment's Translation
/// </Summary>
Segment2D Translation(const Segment2D &seg, const Vector2D &v)
{
	return Segment2D(seg.p0.x + v.x, seg.p0.y + v.y, seg.p1.x + v.x, seg.p1.y + v.y);
}

/// <Summary>
/// 2D Segment's Translation
/// </Summary>
Segment2D Translation(const Segment2D &seg, const Vector2D &v0, const Vector2D &v1)
{
	return Segment2D(seg.p0.x + v0.x, seg.p0.y + v0.y, seg.p1.x + v1.x, seg.p1.y + v1.y);
}

/// <Summary>
/// 2D line's Translation
/// </Summary>
Line2D Translation(const Line2D &line, const Vector2D &v)
{
	return Line2D(line.a, line.b, line.c - line.a * v.x - line.b * v.y);
}

/************************************************************************/
/* Rotation                                                             */
/************************************************************************/
/// <Summary>
/// rotate 2D point counter clockwise around a center point (radian)
/// </Summary>
Point2D Rotation(const Point2D &p, const Point2D &center, double angle)
{
	double _cos = cos(angle);
	double _sin = sin(angle);
	double x = center.x + (p.x - center.x) * _cos - (p.y - center.y) * _sin;
	double y = center.y + (p.x - center.x) * _sin + (p.y - center.y) * _cos;
	return Point2D(x, y);
}

/// <Summary>
/// rotate 2D segment counter clockwise around a center point (radian)
/// </Summary>
Segment2D Rotation(const Segment2D &seg, const Point2D &center, double angle)
{
	return Segment2D(Rotation(seg.p0, center, angle), Rotation(seg.p1, center, angle));
}

/// <Summary>
/// rotate 2D line counter clockwise around a center point (radian)
/// </Summary>
Line2D Rotation(const Line2D &line, const Point2D &center, double angle)
{
	double _cos = cos(angle);
	double _sin = sin(angle);
	double _a = line.a * _cos - line.b * _sin;
	double _b = line.a * _sin + line.b * _cos;
	double _c = line.a * (center.x - center.x * _cos - center.y * _sin) + line.b * (center.y + center.x * _sin - center.y * _cos) + line.c;
	return Line2D(_a, _b, _c);
}


/// <Summary>
/// rotate 2D triangle counter clockwise around a center point (radian)
/// </Summary>
Triangle2D Rotation(const Triangle2D &triangle, const Point2D &center, double angle)
{
	return Triangle2D(
		Rotation(triangle.p0, center, angle),
		Rotation(triangle.p1, center, angle),
		Rotation(triangle.p2, center, angle)
		);
}

/************************************************************************/
/* Merge                                                                */
/************************************************************************/
/// <Summary>
/// merge two rectangle
/// </Summary>
Rectangle2D Merge(const Rectangle2D &rect0, const Rectangle2D &rect1)
{
	return Rectangle2D(
		std::min(rect0.x0, rect1.x0),
		std::min(rect0.y0, rect1.y0),
		std::max(rect0.x1, rect1.x1),
		std::max(rect0.y1, rect1.y1)
		);
}

/************************************************************************/
/* Convert                                                              */
/************************************************************************/
/// <Summary>
/// from 2D point to trilinear coordinate
/// </Summary>
void Convert(const Point2D &point, const Triangle2D &triangle, Trilinear &triCoord)
{
	triCoord.SetValue(point, triangle);
}

/// <Summary>
/// from trilinear coordinate to 2D point
/// </Summary>
void Convert(const Trilinear &triCoord, const Triangle2D &triangle, Point2D &point)
{
	double u0 = triCoord.u0 * Distance(triangle.p1, triangle.p2);
	double u1 = triCoord.u1 * Distance(triangle.p2, triangle.p0);
	double u2 = triCoord.u2 * Distance(triangle.p0, triangle.p1);
	double sum = u0 + u1 + u2;
	point.x = (triangle.p0.x * u0 + triangle.p1.x * u1 + triangle.p2.x * u2) / sum;
	point.y = (triangle.p0.y * u0 + triangle.p1.y * u1 + triangle.p2.y * u2) / sum;
}